// Copyright (c) 2025 Shenzhen Kaihong Digital Industry Development Co., Ltd.
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

use std::{collections::{hash_map::Entry, HashMap}, sync::Arc};

use anyhow::Context;
use futures_util::{stream::SplitSink, SinkExt};
use lazy_static::lazy_static;
use log::{error, trace, warn};
use ohmw_codegen::msg_def::BaseType;
use ohmw_core::bincode::{DefaultEncoder, DefaultEncoderError, Encode};
use ohmw_derive::BorrowDecode;
use rosrust::{subscribe_with_ids_and_headers, RawMessage, Subscriber};
use tokio::{net::TcpStream, sync::Mutex};
use tokio_tungstenite::{tungstenite::Message, WebSocketStream};

use crate::system::{SystemHandles, SYSTEM_HANDLES};

lazy_static! {
    static ref SUBSCRIBER_MAP: Arc<Mutex<HashMap<String, SubscriberInfo>>> = Arc::new(Mutex::new(HashMap::new()));
}

#[derive(BorrowDecode)]
pub struct SubscribeTopicReq<'a> {
    pub topic_name: &'a str,
    pub operate: u8,
}

pub enum SubscribeOperate {
    Stop,
    Start,
    Reply,
}

struct SubscriberInfo {
    _subscriber: Subscriber,
    base_type: Option<BaseType>,
}

impl TryFrom<u8> for SubscribeOperate {
    type Error = anyhow::Error;

    fn try_from(value: u8) -> Result<Self, Self::Error> {
        match value {
            0 => Ok(SubscribeOperate::Stop),
            1 => Ok(SubscribeOperate::Start),
            2 => Ok(SubscribeOperate::Reply),
            _ => Err(anyhow::anyhow!("invalid subscribe operate value: {}", value)),
        }
    }
}

async fn on_message(
    sink: Arc<Mutex<SplitSink<WebSocketStream<TcpStream>, Message>>>,
    topic_name: &str,
    msg: RawMessage,
) {
    fn encode_message(
        buf: &mut Vec<u8>,
        topic_name: &str,
        base_type: &BaseType,
        msg: &RawMessage,
    ) -> Result<(), DefaultEncoderError> {
        let mut encoder = DefaultEncoder::new();
        0u32.encode(&mut encoder, buf)?;
        "subscribe_topic_ret".encode(&mut encoder, buf)?;
        topic_name.encode(&mut encoder, buf)?;
        base_type.string.encode(&mut encoder, buf)?;
        buf.extend_from_slice(msg.0.as_slice());
        (buf.len() as u32 - size_of::<u32>() as u32).encode(&mut DefaultEncoder::new(), buf)?;
        Ok(())
    }

    let map = SUBSCRIBER_MAP.lock().await;
    let subscriber_info = map.get(topic_name);
    let Some(base_type) = subscriber_info.and_then(|s| s.base_type.as_ref()) else {
        error!("msg_type not inited in on_connect for topic {}", topic_name);
        return;
    };

    let mut buf = Vec::new();
    if let Err(e) = encode_message(&mut buf, topic_name, base_type, &msg) {
        error!("failed to encode message: {}", e);
        return;
    }

    if let Err(err) = sink.lock().await.send(Message::Binary(buf.into())).await {
        error!("failed to send message for topic {}: {}", topic_name, err);
    }
}

async fn on_connect(topic_name: &str, headers: HashMap<String, String>) {
    trace!("on_connect for topic {}: headers: {:?}", topic_name, headers);

    let (message_type, message_definition) = match parse_headers(&headers) {
        Ok(result) => result,
        Err(err) => {
            error!("failed to parse message headers: {}", err);
            return;
        },
    };

    let base_type = BaseType::from_string(message_type.to_string());
    if let Some(pkg_name) = base_type.pkg_name() {
        match SYSTEM_HANDLES.lock().await.msg_defs
            .try_add_ros_msg_def(
                pkg_name.to_string(),
                base_type.msg_name().to_string(),
                message_definition,
            )
        {
            Ok(_) => {},
            Err(err) => {
                error!("failed to add message definition for {}/{}: {}", pkg_name, base_type.msg_name(), err);
                SUBSCRIBER_MAP.lock().await.remove(topic_name);
            },
        }
    }

    if let Some(subscriber_info) = SUBSCRIBER_MAP.lock().await.get_mut(topic_name) {
        subscriber_info.base_type = Some(base_type);
    }
}

fn parse_headers(headers: &HashMap<String, String>) -> anyhow::Result<(&str, &str)> {
    let message_type = headers.get("type").context("Missing header `type`")?;
    let message_definition = headers
        .get("message_definition")
        .context("Missing header `message_definition`")?;
    Ok((message_type.as_str(), message_definition.as_str()))
}

pub async fn subscribe_topic(
    sink: Arc<Mutex<SplitSink<WebSocketStream<TcpStream>, Message>>>,
    topic_name: &str,
    queue_size: usize,
    operate: SubscribeOperate,
) {
    if !SystemHandles::ensure_ros_initialized() {
        return;
    }
    if !SYSTEM_HANDLES.lock().await.ensure_msg_defs_scanned().await {
        return;
    }

    match operate {
        SubscribeOperate::Stop => {
            trace!("unsubscribing from topic {}", topic_name);
            if SUBSCRIBER_MAP.lock().await.remove(topic_name).is_none() {
                warn!("topic {} not subscribed", topic_name);
            }
        },
        SubscribeOperate::Start => {
            let mut subscriber_map = SUBSCRIBER_MAP.lock().await;
            match subscriber_map.entry(topic_name.to_string()) {
                Entry::Occupied(_) => {
                    warn!("topic {} already subscribed", topic_name);
                },
                Entry::Vacant(vacant_entry) => {
                    trace!("subscribing to topic {}", topic_name);
                    let topic_name_clone = topic_name.to_string();
                    let topic_name_clone2 = topic_name.to_string();
                    let _subscriber = match subscribe_with_ids_and_headers(
                        topic_name,
                        queue_size,
                        move |msg, _| {
                            futures::executor::block_on(on_message(sink.clone(), &topic_name_clone, msg));
                        },
                        move |headers| {
                            futures::executor::block_on(on_connect(&topic_name_clone2, headers));
                        },
                    ) {
                        Ok(subscriber) => subscriber,
                        Err(err) => {
                            error!("failed to subscribe to topic {}: {}", topic_name, err);
                            return;
                        },
                    };
    
                    vacant_entry.insert(SubscriberInfo { _subscriber, base_type: None });
                },
            }
        },
        SubscribeOperate::Reply => {},
    }
}
